#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/crop_box.h>
#include <pcl/io/ply_io.h>

#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/Image.h>
#include <opencv2/opencv.hpp>

#include<iostream>

class PointCloudFilter {
public:
    PointCloudFilter(ros::NodeHandle& nh) : nh_(nh) {
        // 参数加载
        // nh_.param("min_x", min_x_, -1.0);
        // nh_.param("max_x", max_x_, 1.0);
        // nh_.param("min_y", min_y_, -1.0);
        // nh_.param("max_y", max_y_, 1.0);
        // nh_.param("min_z", min_z_, -1.0);
        // nh_.param("max_z", max_z_, 1.0);

        // 订阅原始点云
        sub_ = nh_.subscribe<sensor_msgs::PointCloud2>("/camera/depth/color/points", 1, &PointCloudFilter::cloudCallback, this);

        sub1_ = nh_.subscribe<sensor_msgs::Image>("/camera/color/image_raw", 1, &PointCloudFilter::imageCallback, this);

        // 发布过滤后的点云
        pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/pub_topic", 1);

        save_counter_ = 0;
    }

    void PointCloudSave(double min_x_, double max_x_, double min_y_, double max_y_, double min_z_, double max_z_)
    {
        if (!cloud) { // 先进行空指针判断，如果不进行空指针判断，程序会出现-段错误 (核心已转储)，即非法访问内存

            ROS_WARN("PointCloud is not initialized.");
            return;
        }

        if (cloud.get()->empty()) {
            ROS_WARN("The point cloud is empty.");
            return;
        }

        pcl::CropBox<pcl::PointXYZRGB> crop_filter; // 创建裁剪器
        crop_filter.setInputCloud(cloud);

        Eigen::Vector4f min_pt(min_x_, min_y_, min_z_, 1.0);
        Eigen::Vector4f max_pt(max_x_, max_y_, max_z_, 1.0);
        crop_filter.setMin(min_pt);
        crop_filter.setMax(max_pt);

        pcl::PointCloud<pcl::PointXYZRGB>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
        crop_filter.filter(*filtered_cloud);

        // 发布过滤后的点云
        // sensor_msgs::PointCloud2 output;
        // pcl::toROSMsg(*filtered_cloud, output);
        // output.header = input->header;
        // pub_.publish(output);

        // 保存为 ply 文件
        std::string filename = "/home/c/ros_ws/pointcloud_" + std::to_string(save_counter_++) + ".ply"; 
        pcl::io::savePLYFile(filename, *filtered_cloud);
        ROS_INFO("Saved filtered point cloud to %s", filename.c_str());
    }

    void saveImage()
    {
        // 转换为RGB格式（如果需要）
        cv::Mat rgb_image;
        cv::cvtColor(cv_ptr->image, rgb_image, cv::COLOR_BGR2RGB);

       
        std::string filename = "/home/c/ros_ws/rgb_" + std::to_string(save_counter_-1) + ".png";
        
        // 保存为PNG格式
        cv::imwrite(filename, rgb_image);
        ROS_INFO("Saved image: %s", filename.c_str());
    }



private:
    void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& input) {
        // 转换为 pcl 点云
	
	 ROS_INFO(" -- Entry cloudCallback funtion! -- ");
        cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
        pcl::fromROSMsg(*input, *cloud); //将ros的消息类型转为点云格式
    }


    void imageCallback(const sensor_msgs::ImageConstPtr& msg) {

        if (msg->encoding == "rgb8") {
            cv_ptr = cv_bridge::toCvCopy(msg, "rgb8"); // 将ROS图像转换为OpenCV格式（BGR8）
        } 
        else if (msg->encoding == "bgr8") {
            cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
        }
    }


    ros::NodeHandle nh_;
    ros::Subscriber sub_, sub1_;
    ros::Publisher pub_;

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
    cv_bridge::CvImagePtr cv_ptr;

    int save_counter_;
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "pointcloud_filter_node");
    ros::NodeHandle nh("~");

    double min_x, max_x, min_y, max_y, min_z, max_z; 

    PointCloudFilter filter(nh);
    
	
	ros::Rate rate(1);
	rate.sleep();
	ros::spinOnce(); // 处理回调，处理完成后再返回这里，类似中断函数

    while (ros::ok()) {
        
        std::string line;
        std::cout <<"输入参数(min_x, max_x, min_y, max_y, min_z, max_z)，或输入 'quit' 退出:";
        std::getline(std::cin, line); // 读取整行输入

        if (line == "quit") {
            break;
        }

        std::istringstream iss(line);
        if (!(iss >> min_x >> max_x >> min_y >> max_y >> min_z >> max_z)) {
            std::cout << "输入格式错误" << std::endl;
            continue;
        }

        std::cout << "min_x: " << min_x << std::endl;
        std::cout << "max_x: " << max_x << std::endl;
        std::cout << "min_y: " << min_y << std::endl;
        std::cout << "max_y: " << max_y << std::endl;
        std::cout << "min_z: " << min_z << std::endl;
        std::cout << "max_z: " << max_z << std::endl;
        
        ros::spinOnce(); // 处理回调，处理完成后再返回这里，类似中断函数
        filter.PointCloudSave(min_x, max_x, min_y, max_y, min_z, max_z);
        filter.saveImage();

    }

    // ros::spin(); //进入一个死循环，一直不断处理回调函数
    // std::cin >> min_x >> max_x >> min_y >> max_y >> min_z >> max_z;
    return 0;
}
